# -*- coding:utf-8 -*-
"""
作者：ChenTao
日期：2023年03月12日

此文件包含主要内容：
    object_detect定义目标检测代码
    conveying_message_to_control取出队列数据，打包数据到主控板
"""
import yaml
import argparse
from util import Yolov5, Control_board

# ---------------------------------------- #
# 加载超config的参数
# ---------------------------------------- #
hym_arg = argparse.ArgumentParser()
hym_arg.add_argument('--yaml', type=str, default='config/pc.yaml', help=argparse.SUPPRESS)
hym_opt = hym_arg.parse_args()
with open(hym_opt.yaml, 'r', encoding='utf-8') as f:
    hym = yaml.load(f.read(), Loader=yaml.SafeLoader)

# ---------------------------------------- #
# 一些全局变量
# ---------------------------------------- #
"""进程间通信数据"""
message = {'coordinate': None, 'angle': None}  # 用于放入队列的数据


def conveying_message_to_control(queen):
    """
    给主控板发消息
    Args:
        queen: 进程之间共享内存的队列

    Returns:

    """

    control_board = Control_board(port=hym['serial']['mpu_com_port'],
                                  baudrate=hym['serial']['baudrate'])  # 初始化与主控板的串口
    try:
        while True:
            data = queen.get()
            try:
                if data['coordinate'] is not None and hym['message']['coordinate']:  # 取出坐标数据
                    # 计算与相机中心点的坐标差
                    dcoordinate = control_board.coordinate_difference(data_tuple=data['coordinate'],
                                                                      translation_vector=hym['capture']['camera'][
                                                                          'translation_vector'],
                                                                      capture_size=hym['capture']['camera']['size'])
                    print('坐标差{}'.format(dcoordinate))
                    control_board.send_message(dcoordinate, pad_len=14)
                if data['angle'] is not None and hym['message']['angle']:  # 取出角度数据
                    print('角度{}'.format(data['angle']))
                    control_board.send_message(data['angle'], pad_len=60)  # 串口发送
            except IOError:
                if control_board:
                    control_board.mpu.com_close()
                control_board = Control_board(port=hym['serial']['mpu_com_port'],
                                              baudrate=hym['serial']['baudrate'])  # 初始化与主控板的串口




    except KeyboardInterrupt:
        if control_board:
            control_board.mpu.com_close()
            print("串口关闭")
        print("程序终止")


def object_detect(queen):
    """目标检测函数"""

    if hym['capture']['video'] == 'D435i' or hym['capture']['video'] == 'd435i':
        """D435摄像头"""
        from yolov5.rstest import Run_D435i_With_YoloV5
        run_d435i_with_yolov5 = Run_D435i_With_YoloV5(weight=hym['yolov5']['weight'],
                                                      data=hym['yolov5']['data'],
                                                      img_size=hym['yolov5']['img_size'],
                                                      view_img=hym['yolov5']['view_img'],
                                                      conf_thres=hym['yolov5']['threshold']['confidence'],
                                                      iou_thres=hym['yolov5']['threshold']['iou'],
                                                      device=hym['yolov5']['device'],
                                                      depth_size=hym['capture']['d435i']['depth_size'],
                                                      color_size=hym['capture']['d435i']['color_size'],
                                                      fps=hym['capture']['d435i']['fps'])
        # 开启检测
        run_d435i_with_yolov5.run(queen, translation_vector=hym['capture']['d435i']['translation_vector'])
    else:
        """普通单目摄像头"""
        cnn = Yolov5(weights_path=hym['yolov5']['weight'],
                     source=hym['capture']['video'],
                     data=hym['yolov5']['data'],
                     img_size=hym['yolov5']['img_size'],
                     cap_info=hym['capture']['camera'],
                     view_img=hym['yolov5']['view_img'],
                     conf_thres=hym['yolov5']['threshold']['confidence'],
                     iou_thres=hym['yolov5']['threshold']['iou'],
                     nosave=hym['yolov5']['nosave'],
                     dnn=hym['yolov5']['dnn'],
                     camera_matrix=hym['capture']['camera']['camera_matrix'],
                     dist_coeffs=hym['capture']['camera']['dist_coeffs'])  # 初始化神经网络

        # 开启检测
        cnn.run_detect(queen)


def take_the_ring(queen):
    gtr = Gtr(view_img=hym['gtr']['view_img'],
              port=hym['gtr']['serial']['port'],
              baudrate=hym['gtr']['serial']['baudrate'],
              video=hym['gtr']['capture']['video'],
              width=hym['gtr']['capture']['width'],
              height=hym['gtr']['capture']['height'],
              zero_x=hym['gtr']['zero_x'],
              zero_y=hym['gtr']['zero_y'],
              rate=hym['gtr']['rate'],
              min_rate=hym['gtr']['min_rate'],
              min_area=hym['gtr']['min_area'])

    gtr.run()


def run__queen(funs):
    """开启多个进程"""

    from multiprocessing import Process, Queue, cpu_count

    # 队列实例化
    queen = Queue(maxsize=8)

    # 定义多个进程，target：函数名，args：函数参数
    process = []
    for fun in funs:
        process.append(Process(target=fun, args=(queen,)))

    assert len(process) <= int(cpu_count()), '进程数多余cpu核数一半，请减少进程'

    # 开启多进程
    [p.start() for p in process]
    [p.join() for p in process]


if __name__ == "__main__":
    # 多进程，通过queen队列通信
    funs = [object_detect,
            conveying_message_to_control,]
    print(str(funs))
    run__queen(funs)
